#include "Prm.h"
#include <boost/foreach.hpp>

#define PRINT_STATUS

Prm::Prm(Polygon_2 &robot_polygon, Obstacles &obstacles, Conf &start, Conf &target, Room &room)
    : m_robot_polygon(robot_polygon), m_obstacles(obstacles), m_start(start), m_target(target), m_room(room),
    m_collision_detector(new CollisionDetector(robot_polygon, obstacles)), m_sampler(robot_polygon, room, m_collision_detector),
    m_graph(0, true), m_kd_tree(), m_first_time(true), m_sould_clear(true)
{
    //insert first the start and target points
    m_vertices.push_back(m_start);
    m_vertices.push_back(m_target);
}

Prm::Prm(Polygon_2 &robot_polygon, Obstacles &obstacles, Conf &start, Conf &target, Room &room, CollisionDetector* collision_detector)
    : m_robot_polygon(robot_polygon), m_obstacles(obstacles), m_start(start), m_target(target), m_room(room),
    m_collision_detector(collision_detector), m_sampler(robot_polygon, room, m_collision_detector),
    m_graph(0, true), m_kd_tree(), m_first_time(true), m_sould_clear(false)
{
    //insert first the start and target points
    m_vertices.push_back(m_start);
    m_vertices.push_back(m_target);
}

Prm::~Prm()
{
    if (m_sould_clear)
    {
        delete m_collision_detector;
    }
}
void Prm::update_path(vector<Conf> &path)
{
    list<Conf> found_path;
    m_graph.find_path(m_start, m_target, found_path);

    list<Conf>::const_iterator iter;
    for(iter = found_path.begin(); iter != found_path.end(); ++iter)
    {
        path.insert(path.end(), *iter);
    }
}

bool Prm::has_path()
{
    list<Conf> found_path;
    m_graph.find_path(m_start, m_target, found_path);

    return found_path.size() >= 2;
}

//	This operation is supposed to generate the roadmap (sample configurations
//	and connect them.
// number_vertices: number of sampled vertices
// k_nearest:maximal degree of vertex in roadmap
void Prm::generate_roadmap(int number_vertices, int	k_nearest, double eps)
{
    //generate number_vertices samples
    vector<Conf> new_vertices;
#ifdef PRINT_STATUS
    std::cout << "  0%";
#endif
    int index = 0;
    for (index = 0; index < number_vertices; ++index)
    {
        Conf val = m_sampler.generate_sample();
        new_vertices.push_back(val);
        m_vertices.push_back(val);
#ifdef PRINT_STATUS
        if (index % 5 == 0)
        {
            int per = (int)(((1.0 * index) / number_vertices) * 100);
            if (per == 100)
            {
                per = 99;
            }
            if (per < 10)
            {
                std::cout << "\x08\x08" << per << "%";
            }
            else
            {
                std::cout << "\x08\x08\x08" << per << "%";
            }
        }
#endif
    }

    vector<Conf>& working_set = m_vertices;
    if (!m_first_time)
    {
        working_set = new_vertices;
    }
    m_first_time = false;

    //Add them to the graph and insert to the kd-tree
    BOOST_FOREACH(Conf& point, working_set)
    {
        m_kd_tree.insert(point);
        m_graph.add_vertex(point);
    }

#ifdef PRINT_STATUS
    std::cout << "\x08\x08\x08" << "100%  0%";
#endif

    index = 0;
    // go over all the points
    BOOST_FOREACH(Conf& p, working_set)
    {
        vector<Conf> nearest_neighbors;
        // find the k- nearest neighbors
        m_kd_tree.k_nearest_neighbors(p, k_nearest, std::back_inserter(nearest_neighbors));
        BOOST_FOREACH(Conf& q, nearest_neighbors){
            // if there isn't an edge between them (or the connected components) and there is a valid-path (by the local planner) between them
            if (!m_graph.is_in_same_cc(p, q) && m_collision_detector->local_planner(p, q, eps))
            {
                // add an edge to the graph
                m_graph.add_edge(p, q);
            }
        }
#ifdef PRINT_STATUS
        if (index % 5 == 0)
        {
            int per = (int)(((1.0 * index) / number_vertices) * 100);
            if (per == 100)
            {
                per = 99;
            }
            if (per < 10)
            {
                std::cout << "\x08\x08" << per << "%";
            }
            else
            {
                std::cout << "\x08\x08\x08" << per << "%";
            }
        }
        ++index;
#endif
    }
#ifdef PRINT_STATUS
    std::cout << "\x08\x08\x08\x08\x08\x08\x08\x08\x08";
#endif
}

Conf Prm::generate_valid_sample()
{
    return m_sampler.generate_sample();
}

Conf Prm::generate_sample()
{
    return m_sampler.generate_random_sample();
}

Conf& Prm::get_neighbor(Conf& sample, Conf target)
{
    vector<Conf*> near = m_graph.get_edged_vertices(sample);
    if (near.size() == 0)
    {
        return sample;
    }
    Conf& best_p = *near.front();
    double best = Metrics::distance(target, best_p);
    for(vector<Conf*>::iterator iter = near.begin(); iter != near.end(); ++iter)
    {
        Conf& current_p = **iter;
        double current = Metrics::distance(target, current_p);
        if (current < best)
        {
            best_p = current_p;
            best = current;
        }
    }
    return best_p;
}

vector<Conf*> Prm::get_neighbors(Conf& sample)
{
    return m_graph.get_edged_vertices(sample);
}
